/*
 * T_Splitsing1.c
 *
 * Created: 24-5-2013 11:09:24
 *  Author: Niels Markhorst
 */ 


#include <avr/io.h>
#include "T-Splitsing1.h"
#include "globals.h"

void TSplitsing1(void)
{

int senArray1[3];
uint8_t andByte;
int staArray1[7][4] =
{
{1,0,0,2},
{0,1,0,4},
{0,0,1,6},
{1,1,1,4},
{1,1,0,2},
{0,1,1,6},
{1,0,1,2}
};


int tellerEen, tellerTwee, tellerDrie, tellerVier=0, tellerVijf=0, tellerZes=0, tellerZeven=0, tellerAcht=0, tellerNegen=0, tellerTien=0, tellerElf=0;

for(int x=0;x<3;x++)
{
	stoplichten1[x]=lichtArray[presentState1-1][x];
}




if (PINA & (1<<0) )
{
	senArray1[0] = 1;
}
else
{
	senArray1[0]= 0;
}

if (PINA & (1<<1))
{
	senArray1[1] = 1;
}
else
{
	senArray1[1]=0;
}

if (PINA & (1<<2))
{
	senArray1[2] = 1;
}
else
{
	senArray1[2]=0;
}




if(presentState1==1)
{
	
	
	
	tellerVier=1;
	
	for(tellerEen=0; tellerEen<7; tellerEen++)
	{
		tellerDrie=0;
		for(tellerTwee=0; tellerTwee<3; tellerTwee++)
		{
			if(senArray1[tellerTwee]==staArray1[tellerEen][tellerTwee])
			{
				tellerDrie++;
				if(tellerDrie==3)
				{
					tellerVier=staArray1[tellerEen][3];
					if(tellerVier!=0 && tellerEen==0)
					{
						tellerVijf=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==1)
					{
						tellerZes=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==2)
					{
						tellerZeven=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==3)
					{
						tellerAcht=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==4)
					{
						tellerNegen=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==5)
					{
						tellerTien=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==6)
					{
						tellerElf=tellerVier;
					}
					
					
				}
			}
		}
	}
}

else if(presentState1==2)
{
	tellerVier=2;
	int staArray2[7][4] =
	{
	{1,0,1,3},
	{0,1,1,3},
	{0,0,0,3},
	{0,0,1,3},
	{0,1,0,3},
	{1,1,0,3},
	{1,1,1,3},
	};
	for(tellerEen=0; tellerEen<7; tellerEen++)
	{
		tellerDrie=0;
		for(tellerTwee=0; tellerTwee<3; tellerTwee++)
		{
			if(senArray1[tellerTwee]==staArray2[tellerEen][tellerTwee])
			{
				tellerDrie++;
				if(tellerDrie==3)
				{
					tellerVier=staArray2[tellerEen][3];
					if(tellerVier!=0 && tellerEen==0)
					{
						tellerVijf=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==1)
					{
						tellerZes=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==2)
					{
						tellerZeven=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==3)
					{
						tellerAcht=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==4)
					{
						tellerNegen=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==5)
					{
						tellerTien=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==5)
					{
						tellerElf=tellerVier;
					}
				}
			}
		}
	}
}

else if(presentState1==3)
{
	
	
	int staArray3[8][4] =
	{
	{0,0,0,1},
	{1,0,0,1},
	{1,1,0,1},
	{1,1,1,1},
	{0,1,1,1},
	{0,0,1,1},
	{0,1,0,1},
	{1,0,1,1},
	};
	for(tellerEen=0; tellerEen<8; tellerEen++)
	{
		tellerDrie=0;
		for(tellerTwee=0; tellerTwee<3; tellerTwee++)
		{
			if(senArray1[tellerTwee]==staArray3[tellerEen][tellerTwee])
			{
				tellerDrie++;
				if(tellerDrie==3)
				{
					tellerVier=staArray3[tellerEen][3];
				}
			}
		}
	}
	
	staArray1[3][3] = 4;
	staArray1[4][3] = 4;
	staArray1[6][3] = 6;
	
}

else if(presentState1==4)
{
	tellerVier=4;
	int staArray4[7][4] =
	{
	{0,1,1,5},
	{1,0,1,5},
	{0,0,0,5},
	{0,0,1,5},
	{1,0,0,5},
	{1,1,0,5},
	{1,1,1,5},
	};
	for(tellerEen=0; tellerEen<7; tellerEen++)
	{
		tellerDrie=0;
		for(tellerTwee=0; tellerTwee<3; tellerTwee++)
		{
			if(senArray1[tellerTwee]==staArray4[tellerEen][tellerTwee])
			{
				tellerDrie++;
				if(tellerDrie==3)
				{
					tellerVier=staArray4[tellerEen][3];
					if(tellerVier!=0 && tellerEen==0)
					{
						tellerVijf=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==1)
					{
						tellerZes=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==2)
					{
						tellerZeven=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==3)
					{
						tellerAcht=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==4)
					{
						tellerNegen=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==5)
					{
						tellerTien=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==5)
					{
						tellerElf=tellerVier;
					}
				}
			}
		}
	}
}

else if(presentState1==5)
{
	
	
	int staArray5[8][4] =
	{
	{0,0,0,1},
	{1,0,0,1},
	{1,1,0,1},
	{1,1,1,1},
	{0,1,1,1},
	{0,0,1,1},
	{0,1,0,1},
	{1,0,1,1},
	};
	for(tellerEen=0; tellerEen<8; tellerEen++)
	{
		tellerDrie=0;
		for(tellerTwee=0; tellerTwee<3; tellerTwee++)
		{
			if(senArray1[tellerTwee]==staArray5[tellerEen][tellerTwee])
			{
				tellerDrie++;
				if(tellerDrie==3)
				{
					tellerVier=staArray5[tellerEen][3];
				}
			}
		}
	}
	
	staArray1[3][3] = 6;
	staArray1[4][3] = 2;
	staArray1[5][3] = 6;
	
}

else if(presentState1==6)
{
	tellerVier=6;
	int staArray6[7][4] =
	{
	{0,1,1,7},
	{1,1,0,7},
	{0,0,0,7},
	{0,1,0,7},
	{1,0,0,7},
	{1,0,1,7},
	{1,1,1,7}
	};
	for(tellerEen=0; tellerEen<7; tellerEen++)
	{
		tellerDrie=0;
		for(tellerTwee=0; tellerTwee<3; tellerTwee++)
		{
			if(senArray1[tellerTwee]==staArray6[tellerEen][tellerTwee])
			{
				tellerDrie++;
				if(tellerDrie==3)
				{
					tellerVier=staArray6[tellerEen][3];
					if(tellerVier!=0 && tellerEen==0)
					{
						tellerVijf=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==1)
					{
						tellerZes=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==2)
					{
						tellerZeven=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==3)
					{
						tellerAcht=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==4)
					{
						tellerNegen=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==5)
					{
						tellerTien=tellerVier;
					}
					else if(tellerVier!=0 && tellerEen==5)
					{
						tellerElf=tellerVier;
					}
				}
			}
		}
	}
}

else if(presentState1==7)
{
	
	
	int staArray7[8][4] =
	{
	{0,0,0,1},
	{1,0,0,1},
	{1,1,0,1},
	{1,1,1,1},
	{0,1,1,1},
	{0,0,1,1},
	{0,1,0,1},
	{1,0,1,1},
	};
	for(tellerEen=0; tellerEen<8; tellerEen++)
	{
		tellerDrie=0;
		for(tellerTwee=0; tellerTwee<3; tellerTwee++)
		{
			if(senArray1[tellerTwee]==staArray7[tellerEen][tellerTwee])
			{
				tellerDrie++;
				if(tellerDrie==3)
				{
					tellerVier=staArray7[tellerEen][3];
				}
			}
		}
	}
	
	staArray1[3][3] = 2;
	staArray1[5][3] = 4;
	staArray1[6][3] = 2;
	
}


if(tellerVier!=0 && tellerVijf==0 && tellerZes==0 && tellerZeven==0 && tellerAcht==0)
{
	presentState1=tellerVier;
}
if(tellerVijf!=0)
{
	presentState1=tellerVijf;
}
else if(tellerZes!=0)
{
	presentState1=tellerZes;
}
else if(tellerZeven!=0)
{
	presentState1=tellerZeven;
}
else if(tellerAcht!=0)
{
	presentState1=tellerAcht;
}
else if(tellerNegen!=0)
{
	presentState1=tellerNegen;
}
else if(tellerTien!=0)
{
	presentState1=tellerTien;
}
else if(tellerElf!=0)
{
	presentState1=tellerElf;
}




if (stoplichten1[0] == 1)
{
	orByteB |= 0b00000010;
}
if (stoplichten1[0] == 2)
{
	orByteB |= 0b00000100;
}
if (stoplichten1[0] == 3)
{
	orByteB |= 0b00001000;
}

//------------------------------------------------------------------------------
if (stoplichten1[1] == 1)
{
	orByteA |=  0b10000000;
}
if (stoplichten1[1] == 2)
{
	orByteA |=  0b01000000;
}
if (stoplichten1[1] == 3)
{
	orByteB |=  0b00000001;
}

//-------------------------------------------------------------------------------
if (stoplichten1[2] == 1)
{
	orByteC |=  0b00010000;
}
if (stoplichten1[2] == 2)
{
	orByteC |=  0b00001000;
}
if (stoplichten1[2] == 3)
{
	orByteC |=  0b00000100;
}


if(orByteB == 0b00001000 || orByteB == 0b00000011 || orByteC == 0b00000100)
{
	vertraging1 = 4;
}
else if(orByteB == 0b00000100 || orByteA == 0b10000000 || orByteC == 0b00001000)
{
	vertraging1 = 2;
}
else
{
	vertraging1 = 1;
}


	}